#include "traffic-sample-v2.h"

namespace traffic_sample_v2
{
	class TrafficSampleVars : public SessionSampleVars
	{
	public:
		SPADAS_VARS(TrafficSample, SessionSampleVars)
		TrafficSampleData data;

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp() = timestamp;

			Array<UInt> offsets(data.elements.size());
			Array<UInt> lengthes(data.elements.size());
			UInt totalDataSize = 0;
			for (UInt i = 0; i < data.elements.size(); i++)
			{
				UInt len = 0;
				switch (data.elements[i].type)
				{
				case TrafficElementType::SpeedLimit:
					len = 6;
					break;
				case TrafficElementType::TrafficSign:
					len = 5;
					break;
				case TrafficElementType::TrafficLight:
					len = 10;
					break;
				case TrafficElementType::StopLine:
					len = 4;
					break;
				case TrafficElementType::SpeedBump:
					len = 4;
					break;
				case TrafficElementType::ArrowMark:
					len = 8;
					break;
				case TrafficElementType::ProhibitionZone:
					len = 7;
					break;
				case TrafficElementType::ParkingSlot:
					len = 7;
					break;
				case TrafficElementType::ZebraCrossing:
					len = 5;
					break;
				default:
					break;
				}
				offsets[i] = totalDataSize;
				lengthes[i] = len;
				totalDataSize += len;
			}

			Array<GeneralElement> values(1 + data.elements.size() * 6 + totalDataSize);
			sample.values() = values;
			UInt base = 1 + data.elements.size() * 6;

			values[0] = (Double)data.elements.size();
			for (UInt i = 0; i < data.elements.size(); i++)
			{
				values[1 + 6 * i] = offsets[i];
				values[2 + 6 * i] = lengthes[i];
				values[3 + 6 * i] = (UInt)data.elements[i].type;
				values[4 + 6 * i] = data.elements[i].id;
				values[5 + 6 * i] = data.elements[i].rawID.valid ? (Double)data.elements[i].rawID.value : OptionalDouble();

				UInt eBase = base + offsets[i];
				switch (data.elements[i].type)
				{
				case TrafficElementType::SpeedLimit:
					values[eBase] = data.elements[i].data.speedLimit.limitedSpeed;
					values[eBase + 1] = convert(data.elements[i].data.speedLimit.posX);
					values[eBase + 2] = convert(data.elements[i].data.speedLimit.posY);
					values[eBase + 3] = convert(data.elements[i].data.speedLimit.posZ);
					break;
				case TrafficElementType::TrafficSign:
					values[eBase] = convert(data.elements[i].data.trafficSign.posX);
					values[eBase + 1] = convert(data.elements[i].data.trafficSign.posY);
					values[eBase + 2] = convert(data.elements[i].data.trafficSign.posZ);
					values[eBase + 3] = (UInt)data.elements[i].data.trafficSign.signType;
					break;
				case TrafficElementType::TrafficLight:
					values[eBase] = (UInt)data.elements[i].data.trafficLight.state;
					values[eBase + 1] = convert(data.elements[i].data.trafficLight.posX);
					values[eBase + 2] = convert(data.elements[i].data.trafficLight.posY);
					values[eBase + 3] = convert(data.elements[i].data.trafficLight.posZ);
					if (data.elements[i].data.trafficLight.arrows.valid)
					{
						values[eBase + 4] = data.elements[i].data.trafficLight.arrows.goStraight ? 1 : 0;
						values[eBase + 5] = data.elements[i].data.trafficLight.arrows.turnLeft ? 1 : 0;
						values[eBase + 6] = data.elements[i].data.trafficLight.arrows.turnRight ? 1 : 0;
						values[eBase + 7] = data.elements[i].data.trafficLight.arrows.uTurn ? 1 : 0;
					}
					values[eBase + 8] = convert(data.elements[i].data.trafficLight.stopPosX);
					values[eBase + 9] = convert(data.elements[i].data.trafficLight.stopPosY);
					break;
				case TrafficElementType::StopLine:
					values[eBase] = data.elements[i].data.stopLine.distance;
					values[eBase + 1] = convert(data.elements[i].data.stopLine.heading);
					break;
				case TrafficElementType::SpeedBump:
					values[eBase] = data.elements[i].data.speedBump.distance;
					values[eBase + 1] = convert(data.elements[i].data.speedBump.heading);
					break;
				case TrafficElementType::ArrowMark:
					values[eBase] = data.elements[i].data.arrowMark.arrows.goStraight ? 1 : 0;
					values[eBase + 1] = data.elements[i].data.arrowMark.arrows.turnLeft ? 1 : 0;
					values[eBase + 2] = data.elements[i].data.arrowMark.arrows.turnRight ? 1 : 0;
					values[eBase + 3] = data.elements[i].data.arrowMark.arrows.uTurn ? 1 : 0;
					values[eBase + 4] = convert(data.elements[i].data.arrowMark.posX);
					values[eBase + 5] = convert(data.elements[i].data.arrowMark.posY);
					break;
				case TrafficElementType::ProhibitionZone:
					values[eBase] = data.elements[i].data.prohibitionZone.centerX;
					values[eBase + 1] = data.elements[i].data.prohibitionZone.centerY;
					values[eBase + 2] = data.elements[i].data.prohibitionZone.width;
					values[eBase + 3] = data.elements[i].data.prohibitionZone.length;
					values[eBase + 4] = convert(data.elements[i].data.prohibitionZone.heading);
					break;
				case TrafficElementType::ParkingSlot:
					values[eBase] = data.elements[i].data.parkingSlot.centerX;
					values[eBase + 1] = data.elements[i].data.parkingSlot.centerY;
					values[eBase + 2] = data.elements[i].data.parkingSlot.width;
					values[eBase + 3] = data.elements[i].data.parkingSlot.length;
					values[eBase + 4] = convert(data.elements[i].data.parkingSlot.heading);
					values[eBase + 5] = (UInt)data.elements[i].data.parkingSlot.occupancy;
					break;
				case TrafficElementType::ZebraCrossing:
					values[eBase] = data.elements[i].data.zebraCrossing.nearDistance;
					values[eBase + 1] = convert(data.elements[i].data.zebraCrossing.farDistance);
					values[eBase + 2] = convert(data.elements[i].data.zebraCrossing.heading);
					break;
				default:
					break;
				}
			}

			sample.significantCount() = values.size();
			return sample;
		}

		Bool fromSample(String protocol, SessionSample sample) override
		{
			if (!sample.is<SessionGeneralSample>()) return FALSE;

			auto generalSample = sample.as<SessionGeneralSample>();
			timestamp = generalSample.timestamp();
			auto values = generalSample.values();

			if (protocol == "traffic-sample-v2")
			{
				if (values.size() < 1) return FALSE;
				if (!values[0].valid) return FALSE;

				Int count = (Int)values[0].value;
				if (values.size() < 1 + 6 * count) return FALSE;

				if (count > 0)
				{
					Int lastOffset = (Int)values[1 + 6 * (count - 1)].value;
					Int lastLength = (Int)values[2 + 6 * (count - 1)].value;
					Int totalDataSize = lastOffset + lastLength;
					if ((Int)values.size() != 1 + 6 * count + totalDataSize) return FALSE;
				}

				ArrayX<TrafficElement> elementsX;
				for (Int i = 0; i < count; i++)
				{
					if (!values[1 + 6 * i].valid || !values[2 + 6 * i].valid || 
						!values[3 + 6 * i].valid || !values[4 + 6 * i].valid) continue;

					auto type = (TrafficElementType)(Int)values[3 + 6 * i].value;
					if (type == TrafficElementType::Invalid) continue;

					Int base = 1 + 6 * count + (Int)values[1 + 6 * i].value;
					Int len = (Int)values[2 + 6 * i].value;
					Int id = (Int)values[4 + 6 * i].value;
					OptionalInt rawID;
					if (values[5 + 6 * i].valid) rawID = (Int)values[5 + 6 * i].value;

					TrafficElement newElem;
					newElem.type = type;
					newElem.id = id;
					newElem.rawID = rawID;

					Bool validElem = TRUE;
					switch (type)
					{
					case TrafficElementType::SpeedLimit:
						if (len != 6) 
						{
							validElem = FALSE;
							break;
						}
						newElem.data.speedLimit.limitedSpeed = (UInt)values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.speedLimit.posX.valid = TRUE;
							newElem.data.speedLimit.posX.value = values[base + 1].value;
						}
						if (values[base + 2].valid)
						{
							newElem.data.speedLimit.posY.valid = TRUE;
							newElem.data.speedLimit.posY.value = values[base + 2].value;
						}
						if (values[base + 3].valid)
						{
							newElem.data.speedLimit.posZ.valid = TRUE;
							newElem.data.speedLimit.posZ.value = values[base + 3].value;
						}
						break;
					case TrafficElementType::TrafficSign:
						if (len != 5)
						{
							validElem = FALSE;
							break;
						}
						newElem.data.trafficSign.signType = values[base + 3].valid ? 
							(TrafficSignType)(Int)values[base + 3].value : TrafficSignType::Stop;
						if (values[base].valid)
						{
							newElem.data.trafficSign.posX.valid = TRUE;
							newElem.data.trafficSign.posX.value = values[base].value;
						}
						if (values[base + 1].valid)
						{
							newElem.data.trafficSign.posY.valid = TRUE;
							newElem.data.trafficSign.posY.value = values[base + 1].value;
						}
						if (values[base + 2].valid)
						{
							newElem.data.trafficSign.posZ.valid = TRUE;
							newElem.data.trafficSign.posZ.value = values[base + 2].value;
						}
						break;
					case TrafficElementType::TrafficLight:
						if (len != 10)
						{
							validElem = FALSE;
							break;
						}
						newElem.data.trafficLight.state = (TrafficLightState)(Int)values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.trafficLight.posX.valid = TRUE;
							newElem.data.trafficLight.posX.value = values[base + 1].value;
						}
						if (values[base + 2].valid)
						{
							newElem.data.trafficLight.posY.valid = TRUE;
							newElem.data.trafficLight.posY.value = values[base + 2].value;
						}
						if (values[base + 3].valid)
						{
							newElem.data.trafficLight.posZ.valid = TRUE;
							newElem.data.trafficLight.posZ.value = values[base + 3].value;
						}
						if (values[base + 4].valid && values[base + 5].valid && 
							values[base + 6].valid && values[base + 7].valid)
						{
							newElem.data.trafficLight.arrows.valid = TRUE;
							newElem.data.trafficLight.arrows.goStraight = values[base + 4].value == 1;
							newElem.data.trafficLight.arrows.turnLeft = values[base + 5].value == 1;
							newElem.data.trafficLight.arrows.turnRight = values[base + 6].value == 1;
							newElem.data.trafficLight.arrows.uTurn = values[base + 7].value == 1;
						}
						if (values[base + 8].valid)
						{
							newElem.data.trafficLight.stopPosX.valid = TRUE;
							newElem.data.trafficLight.stopPosX.value = values[base + 8].value;
						}
						if (values[base + 9].valid)
						{
							newElem.data.trafficLight.stopPosY.valid = TRUE;
							newElem.data.trafficLight.stopPosY.value = values[base + 9].value;
						}
						break;
					case TrafficElementType::StopLine:
						if (len != 4)
						{
							validElem = FALSE;
							break;
						}
						newElem.data.stopLine.distance = values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.stopLine.heading.valid = TRUE;
							newElem.data.stopLine.heading.value = values[base + 1].value;
						}
						break;
					case TrafficElementType::SpeedBump:
						if (len != 4)
						{
							validElem = FALSE;
							break;
						}
						newElem.data.speedBump.distance = values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.speedBump.heading.valid = TRUE;
							newElem.data.speedBump.heading.value = values[base + 1].value;
						}
						break;
					case TrafficElementType::ArrowMark:
						if (len != 8)
						{
							validElem = FALSE;
							break;
						}
						newElem.data.arrowMark.arrows.valid = TRUE;
						newElem.data.arrowMark.arrows.goStraight = values[base].value == 1;
						newElem.data.arrowMark.arrows.turnLeft = values[base + 1].value == 1;
						newElem.data.arrowMark.arrows.turnRight = values[base + 2].value == 1;
						newElem.data.arrowMark.arrows.uTurn = values[base + 3].value == 1;
						if (values[base + 4].valid)
						{
							newElem.data.arrowMark.posX.valid = TRUE;
							newElem.data.arrowMark.posX.value = values[base + 4].value;
						}
						if (values[base + 5].valid)
						{
							newElem.data.arrowMark.posY.valid = TRUE;
							newElem.data.arrowMark.posY.value = values[base + 5].value;
						}
						break;
					case TrafficElementType::ProhibitionZone:
						if (len != 7)
						{
							validElem = FALSE;
							break;
						}
						newElem.data.prohibitionZone.centerX = values[base].value;
						newElem.data.prohibitionZone.centerY = values[base + 1].value;
						newElem.data.prohibitionZone.width = values[base + 2].value;
						newElem.data.prohibitionZone.length = values[base + 3].value;
						if (values[base + 4].valid)
						{
							newElem.data.prohibitionZone.heading.valid = TRUE;
							newElem.data.prohibitionZone.heading.value = values[base + 4].value;
						}
						break;
					case TrafficElementType::ParkingSlot:
						if (len != 7)
						{
							validElem = FALSE;
							break;
						}
						newElem.data.parkingSlot.centerX = values[base].value;
						newElem.data.parkingSlot.centerY = values[base + 1].value;
						newElem.data.parkingSlot.width = values[base + 2].value;
						newElem.data.parkingSlot.length = values[base + 3].value;
						if (values[base + 4].valid)
						{
							newElem.data.parkingSlot.heading.valid = TRUE;
							newElem.data.parkingSlot.heading.value = values[base + 4].value;
						}
						newElem.data.parkingSlot.occupancy = values[base + 5].valid ? 
							(ParkingSlotOccupancy)(Int)values[base + 5].value : ParkingSlotOccupancy::Unknown;
						break;
					case TrafficElementType::ZebraCrossing:
						if (len != 5)
						{
							validElem = FALSE;
							break;
						}
						newElem.data.zebraCrossing.nearDistance = values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.zebraCrossing.farDistance.valid = TRUE;
							newElem.data.zebraCrossing.farDistance.value = values[base + 1].value;
						}
						if (values[base + 2].valid)
						{
							newElem.data.zebraCrossing.heading.valid = TRUE;
							newElem.data.zebraCrossing.heading.value = values[base + 2].value;
						}
						break;
					default:
						validElem = FALSE;
						break;
					}

					if (validElem) elementsX.append(newElem);
				}
				data.elements = elementsX.toArray();
				return TRUE;
			}
			else if (protocol == "traffic-sample-v1")
			{
				if (values.size() < 1) return FALSE;
				if (!values[0].valid) return FALSE;

				Int count = (Int)values[0].value;
				if (values.size() < 1 + 3 * count) return FALSE;

				if (count > 0)
				{
					Int lastOffset = (Int)values[3 * count - 1].value;
					Int lastLength = (Int)values[3 * count].value;
					Int totalDataSize = lastOffset + lastLength;
					if ((Int)values.size() < 1 + 3 * count + totalDataSize) return FALSE;
				}

				ArrayX<TrafficElement> elementsX;
				for (Int i = 0; i < count; i++)
				{
					if (!values[1 + 3 * i].valid || !values[2 + 3 * i].valid) continue;

					auto type = (TrafficElementType)(Int)values[1 + 3 * i].value;
					if (type == TrafficElementType::Invalid) continue;

					Int base = 1 + 3 * count + (Int)values[2 + 3 * i].value;

					TrafficElement newElem;
					newElem.type = type;
					newElem.id = i + 1; // 自动分配ID

					Bool validElem = TRUE;
					switch (type)
					{
					case TrafficElementType::SpeedLimit:
						newElem.data.speedLimit.limitedSpeed = (UInt)values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.speedLimit.posX.valid = TRUE;
							newElem.data.speedLimit.posX.value = values[base + 1].value;
						}
						if (values[base + 2].valid)
						{
							newElem.data.speedLimit.posY.valid = TRUE;
							newElem.data.speedLimit.posY.value = values[base + 2].value;
						}
						break;
					case TrafficElementType::TrafficSign:
						newElem.data.trafficSign.signType = TrafficSignType::Stop;
						if (values[base].valid)
						{
							newElem.data.trafficSign.posX.valid = TRUE;
							newElem.data.trafficSign.posX.value = values[base].value;
						}
						if (values[base + 1].valid)
						{
							newElem.data.trafficSign.posY.valid = TRUE;
							newElem.data.trafficSign.posY.value = values[base + 1].value;
						}
						break;
					case TrafficElementType::TrafficLight:
						newElem.data.trafficLight.state = (TrafficLightState)(Int)values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.trafficLight.posX.valid = TRUE;
							newElem.data.trafficLight.posX.value = values[base + 1].value;
						}
						if (values[base + 2].valid)
						{
							newElem.data.trafficLight.posY.valid = TRUE;
							newElem.data.trafficLight.posY.value = values[base + 2].value;
						}
						if (values[base + 3].valid && values[base + 4].valid && 
							values[base + 5].valid && values[base + 6].valid)
						{
							newElem.data.trafficLight.arrows.valid = TRUE;
							newElem.data.trafficLight.arrows.goStraight = values[base + 3].value == 1;
							newElem.data.trafficLight.arrows.turnLeft = values[base + 4].value == 1;
							newElem.data.trafficLight.arrows.turnRight = values[base + 5].value == 1;
							newElem.data.trafficLight.arrows.uTurn = values[base + 6].value == 1;
						}
						break;
					case TrafficElementType::StopLine:
						newElem.data.stopLine.distance = values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.stopLine.heading.valid = TRUE;
							newElem.data.stopLine.heading.value = values[base + 1].value;
						}
						break;
					case TrafficElementType::SpeedBump:
						newElem.data.speedBump.distance = values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.speedBump.heading.valid = TRUE;
							newElem.data.speedBump.heading.value = values[base + 1].value;
						}
						break;
					case TrafficElementType::ArrowMark:
						newElem.data.arrowMark.arrows.valid = TRUE;
						newElem.data.arrowMark.arrows.goStraight = values[base].value == 1;
						newElem.data.arrowMark.arrows.turnLeft = values[base + 1].value == 1;
						newElem.data.arrowMark.arrows.turnRight = values[base + 2].value == 1;
						newElem.data.arrowMark.arrows.uTurn = values[base + 3].value == 1;
						if (values[base + 4].valid)
						{
							newElem.data.arrowMark.posX.valid = TRUE;
							newElem.data.arrowMark.posX.value = values[base + 4].value;
						}
						if (values[base + 5].valid)
						{
							newElem.data.arrowMark.posY.valid = TRUE;
							newElem.data.arrowMark.posY.value = values[base + 5].value;
						}
						break;
					case TrafficElementType::ProhibitionZone:
						newElem.data.prohibitionZone.centerX = values[base].value;
						newElem.data.prohibitionZone.centerY = values[base + 1].value;
						newElem.data.prohibitionZone.width = values[base + 2].value;
						newElem.data.prohibitionZone.length = values[base + 3].value;
						if (values[base + 4].valid)
						{
							newElem.data.prohibitionZone.heading.valid = TRUE;
							newElem.data.prohibitionZone.heading.value = values[base + 4].value;
						}
						break;
					case TrafficElementType::ParkingSlot:
						newElem.data.parkingSlot.centerX = values[base].value;
						newElem.data.parkingSlot.centerY = values[base + 1].value;
						newElem.data.parkingSlot.width = values[base + 2].value;
						newElem.data.parkingSlot.length = values[base + 3].value;
						if (values[base + 4].valid)
						{
							newElem.data.parkingSlot.heading.valid = TRUE;
							newElem.data.parkingSlot.heading.value = values[base + 4].value;
						}
						break;
					case TrafficElementType::ZebraCrossing:
						newElem.data.zebraCrossing.nearDistance = values[base].value;
						if (values[base + 1].valid)
						{
							newElem.data.zebraCrossing.farDistance.valid = TRUE;
							newElem.data.zebraCrossing.farDistance.value = values[base + 1].value;
						}
						if (values[base + 2].valid)
						{
							newElem.data.zebraCrossing.heading.valid = TRUE;
							newElem.data.zebraCrossing.heading.value = values[base + 2].value;
						}
						break;
					default:
						validElem = FALSE;
						break;
					}

					if (validElem) elementsX.append(newElem);
				}
				data.elements = elementsX.toArray();
				return TRUE;
			}

			return FALSE;
		}

		Bool supportInterpolation() override
		{
			return TRUE;
		}

		SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
		{
			if (!ss1.is<TrafficSample>() || !ss2.is<TrafficSample>()) return SessionSample();

			TrafficSampleData& s1 = ss1.as<TrafficSample>().getVars()->data;
			TrafficSampleData& s2 = ss2.as<TrafficSample>().getVars()->data;
			
			// 检查元素是否有有效ID
			Bool noID = FALSE;
			for (auto elem = s1.elements.firstElem(); elem.valid(); ++elem)
			{
				if (elem->id <= 0)
				{
					noID = TRUE;
					break;
				}
			}
			
			for (auto elem = s2.elements.firstElem(); elem.valid(); ++elem)
			{
				if (elem->id <= 0)
				{
					noID = TRUE;
					break;
				}
			}
			
			// 如果没有有效ID，选择权重更大的那个
			if (noID)
			{
				return w1 > w2 ? ss1 : ss2;
			}
			
			// 创建输出对象
			TrafficSample output;
			output.timestamp() = timestamp;
			TrafficSampleData& so = output.getVars()->data;
			
			// 合并并插值交通元素
			ArrayX<TrafficElement> elementsX;
			
			for (auto s1Elem = s1.elements.firstElem(); s1Elem.valid(); ++s1Elem)
			{
				for (auto s2Elem = s2.elements.firstElem(); s2Elem.valid(); ++s2Elem)
				{
					if (s1Elem->id == s2Elem->id && s1Elem->type == s2Elem->type)
					{
						TrafficElement newElem;
						newElem.id = s1Elem->id;
						newElem.rawID = w1 > w2 ? s1Elem->rawID : s2Elem->rawID;
						newElem.type = s1Elem->type;
						
						Bool validType = TRUE;
						switch (s1Elem->type)
						{
						case TrafficElementType::SpeedLimit:
							newElem.data.speedLimit.limitedSpeed = w1 > w2 ? 
								s1Elem->data.speedLimit.limitedSpeed : s2Elem->data.speedLimit.limitedSpeed;
							
							if (s1Elem->data.speedLimit.posX.valid && s2Elem->data.speedLimit.posX.valid)
							{
								newElem.data.speedLimit.posX.valid = TRUE;
								newElem.data.speedLimit.posX.value = s1Elem->data.speedLimit.posX.value * w1 + 
									s2Elem->data.speedLimit.posX.value * w2;
							}
							else if (s1Elem->data.speedLimit.posX.valid)
							{
								newElem.data.speedLimit.posX = s1Elem->data.speedLimit.posX;
							}
							else if (s2Elem->data.speedLimit.posX.valid)
							{
								newElem.data.speedLimit.posX = s2Elem->data.speedLimit.posX;
							}
							
							if (s1Elem->data.speedLimit.posY.valid && s2Elem->data.speedLimit.posY.valid)
							{
								newElem.data.speedLimit.posY.valid = TRUE;
								newElem.data.speedLimit.posY.value = s1Elem->data.speedLimit.posY.value * w1 + 
									s2Elem->data.speedLimit.posY.value * w2;
							}
							else if (s1Elem->data.speedLimit.posY.valid)
							{
								newElem.data.speedLimit.posY = s1Elem->data.speedLimit.posY;
							}
							else if (s2Elem->data.speedLimit.posY.valid)
							{
								newElem.data.speedLimit.posY = s2Elem->data.speedLimit.posY;
							}
							
							if (s1Elem->data.speedLimit.posZ.valid && s2Elem->data.speedLimit.posZ.valid)
							{
								newElem.data.speedLimit.posZ.valid = TRUE;
								newElem.data.speedLimit.posZ.value = s1Elem->data.speedLimit.posZ.value * w1 + 
									s2Elem->data.speedLimit.posZ.value * w2;
							}
							else if (s1Elem->data.speedLimit.posZ.valid)
							{
								newElem.data.speedLimit.posZ = s1Elem->data.speedLimit.posZ;
							}
							else if (s2Elem->data.speedLimit.posZ.valid)
							{
								newElem.data.speedLimit.posZ = s2Elem->data.speedLimit.posZ;
							}
							break;
							
						case TrafficElementType::TrafficSign:
							newElem.data.trafficSign.signType = w1 > w2 ? 
								s1Elem->data.trafficSign.signType : s2Elem->data.trafficSign.signType;
								
							if (s1Elem->data.trafficSign.posX.valid && s2Elem->data.trafficSign.posX.valid)
							{
								newElem.data.trafficSign.posX.valid = TRUE;
								newElem.data.trafficSign.posX.value = s1Elem->data.trafficSign.posX.value * w1 + 
									s2Elem->data.trafficSign.posX.value * w2;
							}
							else if (s1Elem->data.trafficSign.posX.valid)
							{
								newElem.data.trafficSign.posX = s1Elem->data.trafficSign.posX;
							}
							else if (s2Elem->data.trafficSign.posX.valid)
							{
								newElem.data.trafficSign.posX = s2Elem->data.trafficSign.posX;
							}
							
							if (s1Elem->data.trafficSign.posY.valid && s2Elem->data.trafficSign.posY.valid)
							{
								newElem.data.trafficSign.posY.valid = TRUE;
								newElem.data.trafficSign.posY.value = s1Elem->data.trafficSign.posY.value * w1 + 
									s2Elem->data.trafficSign.posY.value * w2;
							}
							else if (s1Elem->data.trafficSign.posY.valid)
							{
								newElem.data.trafficSign.posY = s1Elem->data.trafficSign.posY;
							}
							else if (s2Elem->data.trafficSign.posY.valid)
							{
								newElem.data.trafficSign.posY = s2Elem->data.trafficSign.posY;
							}
							
							if (s1Elem->data.trafficSign.posZ.valid && s2Elem->data.trafficSign.posZ.valid)
							{
								newElem.data.trafficSign.posZ.valid = TRUE;
								newElem.data.trafficSign.posZ.value = s1Elem->data.trafficSign.posZ.value * w1 + 
									s2Elem->data.trafficSign.posZ.value * w2;
							}
							else if (s1Elem->data.trafficSign.posZ.valid)
							{
								newElem.data.trafficSign.posZ = s1Elem->data.trafficSign.posZ;
							}
							else if (s2Elem->data.trafficSign.posZ.valid)
							{
								newElem.data.trafficSign.posZ = s2Elem->data.trafficSign.posZ;
							}
							break;
							
						case TrafficElementType::TrafficLight:
							newElem.data.trafficLight.state = w1 > w2 ? 
								s1Elem->data.trafficLight.state : s2Elem->data.trafficLight.state;
							newElem.data.trafficLight.arrows = w1 > w2 ? 
								s1Elem->data.trafficLight.arrows : s2Elem->data.trafficLight.arrows;
								
							if (s1Elem->data.trafficLight.posX.valid && s2Elem->data.trafficLight.posX.valid)
							{
								newElem.data.trafficLight.posX.valid = TRUE;
								newElem.data.trafficLight.posX.value = s1Elem->data.trafficLight.posX.value * w1 + 
									s2Elem->data.trafficLight.posX.value * w2;
							}
							else if (s1Elem->data.trafficLight.posX.valid)
							{
								newElem.data.trafficLight.posX = s1Elem->data.trafficLight.posX;
							}
							else if (s2Elem->data.trafficLight.posX.valid)
							{
								newElem.data.trafficLight.posX = s2Elem->data.trafficLight.posX;
							}
							
							if (s1Elem->data.trafficLight.posY.valid && s2Elem->data.trafficLight.posY.valid)
							{
								newElem.data.trafficLight.posY.valid = TRUE;
								newElem.data.trafficLight.posY.value = s1Elem->data.trafficLight.posY.value * w1 + 
									s2Elem->data.trafficLight.posY.value * w2;
							}
							else if (s1Elem->data.trafficLight.posY.valid)
							{
								newElem.data.trafficLight.posY = s1Elem->data.trafficLight.posY;
							}
							else if (s2Elem->data.trafficLight.posY.valid)
							{
								newElem.data.trafficLight.posY = s2Elem->data.trafficLight.posY;
							}
							
							if (s1Elem->data.trafficLight.posZ.valid && s2Elem->data.trafficLight.posZ.valid)
							{
								newElem.data.trafficLight.posZ.valid = TRUE;
								newElem.data.trafficLight.posZ.value = s1Elem->data.trafficLight.posZ.value * w1 + 
									s2Elem->data.trafficLight.posZ.value * w2;
							}
							else if (s1Elem->data.trafficLight.posZ.valid)
							{
								newElem.data.trafficLight.posZ = s1Elem->data.trafficLight.posZ;
							}
							else if (s2Elem->data.trafficLight.posZ.valid)
							{
								newElem.data.trafficLight.posZ = s2Elem->data.trafficLight.posZ;
							}
							
							if (s1Elem->data.trafficLight.stopPosX.valid && s2Elem->data.trafficLight.stopPosX.valid)
							{
								newElem.data.trafficLight.stopPosX.valid = TRUE;
								newElem.data.trafficLight.stopPosX.value = s1Elem->data.trafficLight.stopPosX.value * w1 + 
									s2Elem->data.trafficLight.stopPosX.value * w2;
							}
							else if (s1Elem->data.trafficLight.stopPosX.valid)
							{
								newElem.data.trafficLight.stopPosX = s1Elem->data.trafficLight.stopPosX;
							}
							else if (s2Elem->data.trafficLight.stopPosX.valid)
							{
								newElem.data.trafficLight.stopPosX = s2Elem->data.trafficLight.stopPosX;
							}
							
							if (s1Elem->data.trafficLight.stopPosY.valid && s2Elem->data.trafficLight.stopPosY.valid)
							{
								newElem.data.trafficLight.stopPosY.valid = TRUE;
								newElem.data.trafficLight.stopPosY.value = s1Elem->data.trafficLight.stopPosY.value * w1 + 
									s2Elem->data.trafficLight.stopPosY.value * w2;
							}
							else if (s1Elem->data.trafficLight.stopPosY.valid)
							{
								newElem.data.trafficLight.stopPosY = s1Elem->data.trafficLight.stopPosY;
							}
							else if (s2Elem->data.trafficLight.stopPosY.valid)
							{
								newElem.data.trafficLight.stopPosY = s2Elem->data.trafficLight.stopPosY;
							}
							break;
							
						case TrafficElementType::StopLine:
							newElem.data.stopLine.distance = s1Elem->data.stopLine.distance * w1 + 
								s2Elem->data.stopLine.distance * w2;
								
							if (s1Elem->data.stopLine.heading.valid && s2Elem->data.stopLine.heading.valid)
							{
								newElem.data.stopLine.heading.valid = TRUE;
								newElem.data.stopLine.heading.value = s1Elem->data.stopLine.heading.value * w1 + 
									s2Elem->data.stopLine.heading.value * w2;
							}
							else if (s1Elem->data.stopLine.heading.valid)
							{
								newElem.data.stopLine.heading = s1Elem->data.stopLine.heading;
							}
							else if (s2Elem->data.stopLine.heading.valid)
							{
								newElem.data.stopLine.heading = s2Elem->data.stopLine.heading;
							}
							break;
							
						case TrafficElementType::SpeedBump:
							newElem.data.speedBump.distance = s1Elem->data.speedBump.distance * w1 + 
								s2Elem->data.speedBump.distance * w2;
								
							if (s1Elem->data.speedBump.heading.valid && s2Elem->data.speedBump.heading.valid)
							{
								newElem.data.speedBump.heading.valid = TRUE;
								newElem.data.speedBump.heading.value = s1Elem->data.speedBump.heading.value * w1 + 
									s2Elem->data.speedBump.heading.value * w2;
							}
							else if (s1Elem->data.speedBump.heading.valid)
							{
								newElem.data.speedBump.heading = s1Elem->data.speedBump.heading;
							}
							else if (s2Elem->data.speedBump.heading.valid)
							{
								newElem.data.speedBump.heading = s2Elem->data.speedBump.heading;
							}
							break;
							
						case TrafficElementType::ArrowMark:
							newElem.data.arrowMark.arrows = w1 > w2 ? 
								s1Elem->data.arrowMark.arrows : s2Elem->data.arrowMark.arrows;
								
							if (s1Elem->data.arrowMark.posX.valid && s2Elem->data.arrowMark.posX.valid)
							{
								newElem.data.arrowMark.posX.valid = TRUE;
								newElem.data.arrowMark.posX.value = s1Elem->data.arrowMark.posX.value * w1 + 
									s2Elem->data.arrowMark.posX.value * w2;
							}
							else if (s1Elem->data.arrowMark.posX.valid)
							{
								newElem.data.arrowMark.posX = s1Elem->data.arrowMark.posX;
							}
							else if (s2Elem->data.arrowMark.posX.valid)
							{
								newElem.data.arrowMark.posX = s2Elem->data.arrowMark.posX;
							}
							
							if (s1Elem->data.arrowMark.posY.valid && s2Elem->data.arrowMark.posY.valid)
							{
								newElem.data.arrowMark.posY.valid = TRUE;
								newElem.data.arrowMark.posY.value = s1Elem->data.arrowMark.posY.value * w1 + 
									s2Elem->data.arrowMark.posY.value * w2;
							}
							else if (s1Elem->data.arrowMark.posY.valid)
							{
								newElem.data.arrowMark.posY = s1Elem->data.arrowMark.posY;
							}
							else if (s2Elem->data.arrowMark.posY.valid)
							{
								newElem.data.arrowMark.posY = s2Elem->data.arrowMark.posY;
							}
							break;
							
						case TrafficElementType::ProhibitionZone:
							newElem.data.prohibitionZone.centerX = s1Elem->data.prohibitionZone.centerX * w1 + 
								s2Elem->data.prohibitionZone.centerX * w2;
							newElem.data.prohibitionZone.centerY = s1Elem->data.prohibitionZone.centerY * w1 + 
								s2Elem->data.prohibitionZone.centerY * w2;
							newElem.data.prohibitionZone.width = s1Elem->data.prohibitionZone.width * w1 + 
								s2Elem->data.prohibitionZone.width * w2;
							newElem.data.prohibitionZone.length = s1Elem->data.prohibitionZone.length * w1 + 
								s2Elem->data.prohibitionZone.length * w2;
								
							if (s1Elem->data.prohibitionZone.heading.valid && s2Elem->data.prohibitionZone.heading.valid)
							{
								newElem.data.prohibitionZone.heading.valid = TRUE;
								newElem.data.prohibitionZone.heading.value = s1Elem->data.prohibitionZone.heading.value * w1 + 
									s2Elem->data.prohibitionZone.heading.value * w2;
							}
							else if (s1Elem->data.prohibitionZone.heading.valid)
							{
								newElem.data.prohibitionZone.heading = s1Elem->data.prohibitionZone.heading;
							}
							else if (s2Elem->data.prohibitionZone.heading.valid)
							{
								newElem.data.prohibitionZone.heading = s2Elem->data.prohibitionZone.heading;
							}
							break;
							
						case TrafficElementType::ParkingSlot:
							newElem.data.parkingSlot.centerX = s1Elem->data.parkingSlot.centerX * w1 + 
								s2Elem->data.parkingSlot.centerX * w2;
							newElem.data.parkingSlot.centerY = s1Elem->data.parkingSlot.centerY * w1 + 
								s2Elem->data.parkingSlot.centerY * w2;
							newElem.data.parkingSlot.width = s1Elem->data.parkingSlot.width * w1 + 
								s2Elem->data.parkingSlot.width * w2;
							newElem.data.parkingSlot.length = s1Elem->data.parkingSlot.length * w1 + 
								s2Elem->data.parkingSlot.length * w2;
							newElem.data.parkingSlot.occupancy = w1 > w2 ? 
								s1Elem->data.parkingSlot.occupancy : s2Elem->data.parkingSlot.occupancy;
								
							if (s1Elem->data.parkingSlot.heading.valid && s2Elem->data.parkingSlot.heading.valid)
							{
								newElem.data.parkingSlot.heading.valid = TRUE;
								newElem.data.parkingSlot.heading.value = s1Elem->data.parkingSlot.heading.value * w1 + 
									s2Elem->data.parkingSlot.heading.value * w2;
							}
							else if (s1Elem->data.parkingSlot.heading.valid)
							{
								newElem.data.parkingSlot.heading = s1Elem->data.parkingSlot.heading;
							}
							else if (s2Elem->data.parkingSlot.heading.valid)
							{
								newElem.data.parkingSlot.heading = s2Elem->data.parkingSlot.heading;
							}
							break;
							
						case TrafficElementType::ZebraCrossing:
							newElem.data.zebraCrossing.nearDistance = s1Elem->data.zebraCrossing.nearDistance * w1 + 
								s2Elem->data.zebraCrossing.nearDistance * w2;
								
							if (s1Elem->data.zebraCrossing.farDistance.valid && s2Elem->data.zebraCrossing.farDistance.valid)
							{
								newElem.data.zebraCrossing.farDistance.valid = TRUE;
								newElem.data.zebraCrossing.farDistance.value = s1Elem->data.zebraCrossing.farDistance.value * w1 + 
									s2Elem->data.zebraCrossing.farDistance.value * w2;
							}
							else if (s1Elem->data.zebraCrossing.farDistance.valid)
							{
								newElem.data.zebraCrossing.farDistance = s1Elem->data.zebraCrossing.farDistance;
							}
							else if (s2Elem->data.zebraCrossing.farDistance.valid)
							{
								newElem.data.zebraCrossing.farDistance = s2Elem->data.zebraCrossing.farDistance;
							}
							
							if (s1Elem->data.zebraCrossing.heading.valid && s2Elem->data.zebraCrossing.heading.valid)
							{
								newElem.data.zebraCrossing.heading.valid = TRUE;
								newElem.data.zebraCrossing.heading.value = s1Elem->data.zebraCrossing.heading.value * w1 + 
									s2Elem->data.zebraCrossing.heading.value * w2;
							}
							else if (s1Elem->data.zebraCrossing.heading.valid)
							{
								newElem.data.zebraCrossing.heading = s1Elem->data.zebraCrossing.heading;
							}
							else if (s2Elem->data.zebraCrossing.heading.valid)
							{
								newElem.data.zebraCrossing.heading = s2Elem->data.zebraCrossing.heading;
							}
							break;
						
						default:
							validType = FALSE;	
							break;
						}
						
						if (validType) elementsX.append(newElem);
						break;
					}
				}
			}
			
			so.elements = elementsX.toArray();
			return output.as<SessionSample>();
		}

		static OptionalDouble convert(OptBool val)
		{
			return val.valid ? (val.value ? 1 : 0) : OptionalDouble();
		}
		static OptionalDouble convert(OptDouble val)
		{
			return val.valid ? val.value : OptionalDouble();
		}
	};
}

using namespace traffic_sample_v2;

TrafficSample::TrafficSample() : Object<TrafficSampleVars>(new TrafficSampleVars, TRUE)
{}

FullTimestamp& TrafficSample::timestamp()
{
	return vars->timestamp;
}

TrafficSampleData *TrafficSample::operator ->()
{
    return &vars->data;
}

String TrafficSample::getProtocolName(UInt channel)
{
    return (String)"traffic-sample-v2@" + channel;
}

SessionGeneralSample TrafficSample::toGeneralSample()
{
    return vars->toGeneralSample();
}

Array<String> TrafficSample::getProtocolNames(UInt channel)
{
	return Array<String>::create(2, "traffic-sample-v1", "traffic-sample-v2").convert<String>([channel](auto& s){ return s + "@" + channel; });
}

Bool TrafficSample::fromGeneralSample(String protocol, SessionGeneralSample generalSample)
{
    return vars->fromSample(protocol, generalSample.as<SessionSample>());
}